#include "mpu6050device.h"
#include "log/logger.h"
#include "MPU6050/atk_ms6050.h"
#include "MPU6050/eMPL/inv_mpu.h"
#include "os/mutexlocker.h"

static GpioPortPin m_SCLPortPinContext;
static GpioPortPin m_SDAPortPinContext;
static GpioPortPin m_ADPortPinContext;

//context 注入
extern "C"
{
    //模拟IIC
    void MPU6050_WriteSCL(int v)
    {
        if(m_SCLPortPinContext.IsNone())
            return;
        SOC_Gpio::Write(m_SCLPortPinContext, v);
    }

    void MPU6050_WriteSDA(int v)
    {
        if(m_SDAPortPinContext.IsNone())
            return;
        SOC_Gpio::Write(m_SDAPortPinContext, v);
    }

    void MPU6050_WriteAD(int v)
    {
        if(m_ADPortPinContext.IsNone())
            return;
        SOC_Gpio::Write(m_ADPortPinContext, v);
    }

    int MPU6050_ReadSDA()
    {  
        if(m_SDAPortPinContext.IsNone())
            return -1;
        return SOC_Gpio::Read(m_SDAPortPinContext);
    }

    //硬件IIC
    void MPU6050_SOC_IIC_Write(uint8_t dat)
    {  
        
    }

    uint8_t MPU6050_SOC_IIC_Read()
    {  
        return 1;
    }
}

MPU6050Device::MPU6050Device()
{
}

MPU6050Device::~MPU6050Device()
{
}

void MPU6050Device::ConfigEmuIIC(const GpioPortPin &scl, const GpioPortPin &sda)
{
    m_SCLPortPin = scl;
    m_SDAPortPin = sda;
}

void MPU6050Device::ConfigHardware(const GpioPortPin &ad, GpioExtiDevId extiId)
{
    m_ADPortPin = ad;
    m_exti.InitDevId(extiId);
}

void MPU6050Device::Config(MPU6050IICMode iicMode, MPU6050CalcMode calcMode, int rate, bool enInterrupt)
{    
    m_iicMode = iicMode;
    m_calcMode = calcMode;
    m_sampleRate = rate;
    m_interrupt = enInterrupt;
}

void MPU6050Device::SetInterruptCallBack(const INT_FUNC &func)
{
    m_exti.SetExtiCallBack([=](auto &sender){
        FW_UNUSED(sender);
        func();
    });
}

bool MPU6050Device::Init()
{
    MutexLocker locker(&m_mutex);
    uint8_t ret;

    SwitchContext();

    //初始化算法
    m_mahonyAlg.begin(m_sampleRate);
    m_madgwickAlg.begin(m_sampleRate);

    /* 初始化*/
    ret = atk_ms6050_init(m_sampleRate, m_interrupt == true? 1:0);
    if (ret != 0)
    {
        LogDebug()<<("MPU6050 init failed!\r\n");
        return false;
    }
    
    /* 初始化DMP */
    if(m_calcMode == MPU6050CalcMode::DMP)
    {
        ret = atk_ms6050_dmp_init(m_sampleRate);
        if (ret != 0)
        {
            LogDebug()<<("MPU6050 DMP init failed!\r\n");
            return false;
        }
    }

    return true;
}

bool MPU6050Device::GetResult(float &pitch, float &rol, float &yaw)
{
    MutexLocker locker(&m_mutex);
    uint8_t ret;

    SwitchContext();

    if(m_calcMode == MPU6050CalcMode::DMP)
    {
        ret  = atk_ms6050_dmp_get_data(&pitch, &rol, &yaw);
    }
    else
    {
        ret = GetResultNoDmpImpl(pitch, rol, yaw);
    }

    return ret == 0;
}

bool MPU6050Device::GetResult(MPU6050Result &result, bool readAcc, bool readGyr, bool readTemp)
{
    MutexLocker locker(&m_mutex);
    uint8_t ret;

    SwitchContext();

    /* 获取ATK-MS6050 DMP处理后的数据 */
    ret  = atk_ms6050_dmp_get_data(&result.pitch, &result.rol, &result.yaw);
    /* 获取ATK-MS6050加速度值 */
    if(readAcc)
        ret += atk_ms6050_get_accelerometer(&result.acc_x, &result.acc_y, &result.acc_z);
    /* 获取ATK-MS6050陀螺仪值 */
    if(readGyr)
        ret += atk_ms6050_get_gyroscope(&result.gyr_x, &result.gyr_y, &result.gyr_z);
    /* 获取ATK-MS6050温度值 */
    if(readTemp)
        ret += atk_ms6050_get_temperature(&result.temp);

    result.isOk = (ret == 0);
    return result.isOk;
}

bool MPU6050Device::GetAcc(int16_t &accX, int16_t &accY, int16_t &accZ)
{
    MutexLocker locker(&m_mutex);
    uint8_t ret;

    SwitchContext();

    ret = atk_ms6050_get_accelerometer(&accX, &accY, &accZ);
    return ret == 0;
}

bool MPU6050Device::GetGry(int16_t &gyrX, int16_t &gyrY, int16_t &gyrZ)
{
    MutexLocker locker(&m_mutex);
    uint8_t ret;

    SwitchContext();

    ret = atk_ms6050_get_gyroscope(&gyrX, &gyrY, &gyrZ);
    return ret == 0;
}

bool MPU6050Device::GetTemp(float &temp)
{
    MutexLocker locker(&m_mutex);
    uint8_t ret;

    SwitchContext();
    
    int16_t val = 0;
    ret = atk_ms6050_get_temperature(&val);
    if(ret == 0)
    {
        temp = val / 100.f;
    }
    return ret == 0;
}

float MPU6050Device::ConvertRawAcceleration(int aRaw)
{
    // since we are using 2 g range
    // -2 g maps to a raw value of -32768
    // +2 g maps to a raw value of 32767
    
    float a = (aRaw * 2.0) / 32768.0;
    return a;
}

float MPU6050Device::ConvertRawGyro(int gRaw)
{
    // since we are using 250 degrees/seconds range
    // -250 maps to a raw value of -32768
    // +250 maps to a raw value of 32767
    
    // float g = (gRaw * 250.0) / 32768.0;
    float g = (gRaw * 2000.0) / 32768.0;
    return g;
}

void MPU6050Device::SwitchContext()
{
    if(m_iicMode == MPU6050IICMode::IIC_Emu)
    {
        m_SCLPortPinContext = m_SCLPortPin;
        m_SDAPortPinContext = m_SDAPortPin;
    }

    m_ADPortPinContext = m_ADPortPin;
}

bool MPU6050Device::GetResultNoDmpImpl(float &pitch, float &rol, float &yaw)
{
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
    uint8_t ret;

    ret = atk_ms6050_get_accelerometer(&ax, &ay, &az);
    if(ret != 0)
        return false;

    ret = atk_ms6050_get_gyroscope(&gx, &gy, &gz);
    if(ret != 0)
        return false;

    ax = ConvertRawAcceleration(ax);
    ay = ConvertRawAcceleration(ay);
    az = ConvertRawAcceleration(az);
    
    gx = ConvertRawGyro(gx);
    gy = ConvertRawGyro(gy);
    gz = ConvertRawGyro(gz);

    if(ret == 0)
    {
        switch (m_calcMode)
        {
        case MPU6050CalcMode::Mahony:
            m_mahonyAlg.updateIMU(gx, gy, gz, ax, ay, az);
            pitch = m_mahonyAlg.getPitch();
            rol = m_mahonyAlg.getRoll();
            yaw = m_mahonyAlg.getYaw();
            break;
        case MPU6050CalcMode::Madgwick:
            m_madgwickAlg.updateIMU(gx, gy, gz, ax, ay, az);
            pitch = m_madgwickAlg.getPitch();
            rol = m_madgwickAlg.getRoll();
            yaw = m_madgwickAlg.getYaw();
            break;
        default:
            return false; 
            break;
        }
    }

    return true;
}
